#include "BLEHIDDevice.h"
#include "BLEHIDKeyboard.h"
#include "BLEDevice.h"
#include "WiFi.h"
#include "StreamIO.h"
#include "VideoStream.h"
#include "RTSP.h"
#include "NNFaceDetectionRecognition.h"
#include "VideoStreamOverlay.h"
#include <AmebaServo.h>
#include "AmebaFatFS.h"

char filename[] = "pw.txt";
String password = "";

#define CHANNELVID  0    // Channel for RTSP streaming
#define CHANNELJPEG 1    // Channel for taking snapshots
#define CHANNELNN   3    // RGB format video for NN only available on channel 3

// Customised resolution for NN
#define NNWIDTH  576
#define NNHEIGHT 320

VideoSetting configVID(VIDEO_FHD, 30, VIDEO_H264, 0);
VideoSetting configJPEG(VIDEO_FHD, CAM_FPS, VIDEO_JPEG, 1);
VideoSetting configNN(NNWIDTH, NNHEIGHT, 10, VIDEO_RGB, 0);
NNFaceDetectionRecognition facerecog;
RTSP rtsp;
StreamIO videoStreamer(1, 1);
StreamIO videoStreamerFDFR(1, 1);
StreamIO videoStreamerRGBFD(1, 1);

char ssid[] = "测试外网";    // your network SSID (name)
char pass[] = "tengyunceshiwai123";        // your network password
int status = WL_IDLE_STATUS;

bool unLock = false;
uint32_t img_addr = 0;
uint32_t img_len = 0;
long counter = 0;

// File Initialization
AmebaFatFS fs;

char buf[128];
char path[128];

BLEHIDKeyboard keyboardDev;
BLEAdvertData advdata;

void setup() {

  fs.begin();
  printf("write something to \"%s\"\r\n", filename);
  sprintf(path, "%s%s", fs.getRootPath(), filename);
  File file = fs.open(path);

  password = file.readString();

  file.close();
  printf("==== content ====\r\n");
  printf("%s", buf);
  printf("====   end   ====\r\n");
  fs.end();

  Serial.begin(115200);
  advdata.addFlags();
  advdata.addCompleteName("AMEBA_HELLO");
  advdata.addAppearance(GAP_GATT_APPEARANCE_HUMAN_INTERFACE_DEVICE);
  advdata.addCompleteServices(BLEUUID(HID_SERVICE_UUID));

  BLEHIDDev.init();

  BLE.init();
  BLE.configAdvert()->setAdvData(advdata);
  BLE.setDeviceName("AMEBA_HELLO");
  BLE.setDeviceAppearance(GAP_GATT_APPEARANCE_HUMAN_INTERFACE_DEVICE);
  BLE.configSecurity()->setPairable(true);
  BLE.configSecurity()->setAuthFlags(GAP_AUTHEN_BIT_BONDING_FLAG);
  BLE.configServer(3);
  BLE.addService(BLEHIDDev.hidService());
  BLE.addService(BLEHIDDev.battService());
  BLE.addService(BLEHIDDev.devInfoService());
  BLE.beginPeripheral();

  // Attempt to connect to Wifi network:
  while (status != WL_CONNECTED) {
      Serial.print("Attempting to connect to WPA SSID: ");
      Serial.println(ssid);
      status = WiFi.begin(ssid, pass);

      // wait 2 seconds for connection:
      delay(2000);
  }

  // Configure camera video channels with video format information
  Camera.configVideoChannel(CHANNELVID, configVID);
  Camera.configVideoChannel(CHANNELJPEG, configJPEG);
  Camera.configVideoChannel(CHANNELNN, configNN);
  Camera.videoInit();

  // Configure RTSP with corresponding video format information
  rtsp.configVideo(configVID);
  rtsp.begin();

  // Configure Face Recognition model
  facerecog.configVideo(configNN);
  facerecog.modelSelect(FACE_RECOGNITION, NA_MODEL, DEFAULT_SCRFD, DEFAULT_MOBILEFACENET);
  facerecog.begin();
  facerecog.setResultCallback(FRPostProcess);

  // Configure StreamIO object to stream data from video channel to RTSP
  videoStreamer.registerInput(Camera.getStream(CHANNELVID));
  videoStreamer.registerOutput(rtsp);
  if (videoStreamer.begin() != 0) {
      Serial.println("StreamIO link start failed");
  }

  // Start data stream from video channel
  Camera.channelBegin(CHANNELVID);
  Camera.channelBegin(CHANNELJPEG);

  // Configure StreamIO object to stream data from RGB video channel to face detection
  videoStreamerRGBFD.registerInput(Camera.getStream(CHANNELNN));
  videoStreamerRGBFD.setStackSize();
  videoStreamerRGBFD.setTaskPriority();
  videoStreamerRGBFD.registerOutput(facerecog);
  if (videoStreamerRGBFD.begin() != 0) {
      Serial.println("StreamIO link start failed");
  }

  // Start video channel for NN
  Camera.channelBegin(CHANNELNN);

  // Start OSD drawing on RTSP video channel
  OSD.configVideo(CHANNELVID, configVID);
  OSD.begin();

  // Restore any registered faces saved in flash
  facerecog.restoreRegisteredFace();
}

void loop() {

  if (Serial.available() > 0) {
    String input = Serial.readString();
    input.trim();

    if (input.startsWith(String("REG="))) {
        String name = input.substring(4);
        facerecog.registerFace(name);
    } else if (input.startsWith(String("DEL="))) {
        String name = input.substring(4);
        facerecog.removeFace(name);
    } else if (input.startsWith(String("RESET"))) {
        facerecog.resetRegisteredFace();
    } else if (input.startsWith(String("BACKUP"))) {
        facerecog.backupRegisteredFace();
    } else if (input.startsWith(String("RESTORE"))) {
        facerecog.restoreRegisteredFace();
    } else if(input.startsWith(String("PW="))){
      password = input.substring(3);
      File file = fs.open(path);
      file.println(password);
      file.close();
      printf("write finish\r\n\r\n");
    }
  }

  if ((unLock == true) && BLE.connected()) {
      unLock = false;
      Serial.println("Sending keystrokes");
      keyboardDev.keyReleaseAll();
      delay(100);
      keyboardDev.keyPress(HID_KEY_ESCAPE);
      delay(300);
      keyboardDev.keyReleaseAll();
      delay(300);
      keyboardDev.keySequence("123456");
      delay(100);
      keyboardDev.keyPress(HID_KEY_ENTER);
      delay(100);
      keyboardDev.keyReleaseAll();
  }

  delay(1000);
  OSD.createBitmap(CHANNELVID);
  OSD.update(CHANNELVID);

}


// User callback function for post processing of face recognition results
void FRPostProcess(std::vector<FaceRecognitionResult> results)
{
    uint16_t im_h = configVID.height();
    uint16_t im_w = configVID.width();

    printf("Total number of faces detected = %d\r\n", facerecog.getResultCount());
    OSD.createBitmap(CHANNELVID);

    if (facerecog.getResultCount() > 0 && !unLock) {
     
        if (facerecog.getResultCount() > 1) {    // Door remain close when more than one face detected
            unLock = false;
        } else {
            FaceRecognitionResult face = results[0];
            if (String(face.name()) == String("unknown")) {    // Door remain close when unknown face detected
                unLock = false;
            } else {    // Door open when a single registered face detected
                unLock = true;
            }
        }
        
    }

    for (int i = 0; i < facerecog.getResultCount(); i++) {
        FaceRecognitionResult item = results[i];
        // Result coordinates are floats ranging from 0.00 to 1.00
        // Multiply with RTSP resolution to get coordinates in pixels
        int xmin = (int)(item.xMin() * im_w);
        int xmax = (int)(item.xMax() * im_w);
        int ymin = (int)(item.yMin() * im_h);
        int ymax = (int)(item.yMax() * im_h);

        uint32_t osd_color;

        // Draw boundary box
        if (String(item.name()) == String("unknown")) {
            osd_color = OSD_COLOR_RED;
        } else {
            osd_color = OSD_COLOR_GREEN;
        }
        printf("Face %d name %s:\t%d %d %d %d\n\r", i, item.name(), xmin, xmax, ymin, ymax);
        OSD.drawRect(CHANNELVID, xmin, ymin, xmax, ymax, 3, osd_color);

        // Print identification text above boundary box
        char text_str[40];
        snprintf(text_str, sizeof(text_str), "Face:%s", item.name());
        OSD.drawText(CHANNELVID, xmin, ymin - OSD.getTextHeight(CHANNELVID), text_str, osd_color);
    }
    OSD.update(CHANNELVID);
}
